%clear all
close all
fprintf('\nStiffness script START\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
log_name = './logs/test';
tc = 0.18;
gr = 17.25;
A2T = tc*gr;


n = 1001;

% leg_angle		= [];
% trans_angle		= [];
% motor_current	= [];

t_start = 1;
t_end	= 2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run controllers


trq = linspace(15,-15,n);

% for test_idx = 1:length(trq)
% 	
% 	fprintf('Running controller:\n');
% 	fprintf('Command Torque: %f\n',trq(test_idx));
% 	%			controller	log_file	time_start	time_stop	[controller_parms]
% 	cmd_str = sprintf('./bin/fca_interface_headless 1 %s.%d.log %d %d %f',log_name,test_idx, t_start, t_end, trq(test_idx));
% 	
% 	ret = unix(cmd_str);
% 	if (ret ~= 0)
% 		fprintf('Controller fail!\n');
% 		return;
% 	end
% 	
% 	
% 	pause(0.5);
% 
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

% fprintf('Compiling logs\n');
% 
% for test_idx = 1:length(trq)
% 	fprintf('Opening log:\t');
% 	fprintf('%s.%d.log\n',log_name,test_idx);
% 	
% 	log_str = sprintf('%s.%d.log',log_name,test_idx);
% 	log = importdata(log_str);
% 		
% 	motor_angle(:,test_idx)		= log.data(:,4);
% 	leg_angle(:,test_idx)		= log.data(:,5);
% 	
% 	motor_current(:,test_idx)	= log.data(:,8);
% end

trq = mean(motor_current*A2T);

tmp = polyfit(trq,mean(motor_angle - leg_angle),1);
off = -tmp(2);
% off = (leg_angle(end/2) - motor_angle(end/2));

dtheta = mean(motor_angle + off - leg_angle);
tmp = polyfit(trq,dtheta,1);


k_est	= 1/tmp(1);
fprintf('\n\nK = %f,  offset = %f\n',k_est,off)

% figure
% hold on
% % plot(motor_angle+off,'b','linewidth',2);
% plot(leg_angle,'k','linewidth',2);


figure
hold on

plot(trq, mean(motor_angle - leg_angle),'b','linewidth',3)
plot(trq, trq*tmp(1),'r','linewidth',2)
plot(trq, trq*(1/-108),'g','linewidth',2)


return
